package com.googlecode.curvedmetromaps.core.logic.forcedriven;

import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;

import com.googlecode.curvedmetromaps.core.geometry.BasicGeometry;
import com.googlecode.curvedmetromaps.core.geometry.BezierUtil;
import com.googlecode.curvedmetromaps.core.logic.costfunction.CostConstants;
import com.googlecode.curvedmetromaps.core.model.MetroEdge;
import com.googlecode.curvedmetromaps.core.model.MetroStation;

/**
 * Constraint S4: forces that push edges apart.
 */
public class ForceRepulsiveEdges extends ForceSubsystem {
	public ForceWeight maxForce = new ForceWeight("Maximum force", 5000);
	public ForceWeight minForce = new ForceWeight("Cutoff force", 500);

	private double calcCount1, calcCount2, calcCount3;
	
	ForceRepulsiveEdges(ForceSystem system) {
		super(system);
		baseWeight.value = 500;
	}

	void applyForces(int delta) {
		for (MetroStation m : system.metroMap.stations) {
			m.edgeProj.clear();
		}
		
		float minDistSq = (float) (baseWeight.value / minForce.value);
		float minDist = (float) Math.sqrt(minDistSq);

		for (MetroEdge e : system.metroMap.edgeArray) {
			Body ebody0 = system.stationBodies[e.src.id];
			Body ebody1 = system.stationBodies[e.dst.id];
			Vec2 v0 = ebody0.getPosition();
			Vec2 v1 = ebody1.getPosition();
			Vec2 cp = system.controlPointBodies[e.id].getPosition();
			
			// Calculate bounding box that stations must be in.
			float xMin = v0.x; 
			float yMin = v0.y; 
			float xMax = v0.x; 
			float yMax = v0.y;
			Vec2[] vectors = { v1, cp };
			for (Vec2 v : vectors) {
				if (v.x < xMin)
					xMin = v.x;
				else if (v.x > xMax)
					xMax = v.x;

				if (v.y < yMin)
					yMin = v.y;
				else if (v.x > xMax)
					xMax = v.x;
			}
			
			// Enlarge bounding box
			xMin -= minDist;
			yMin -= minDist;
			xMax += minDist;
			yMax += minDist;
			
			for (MetroStation m : system.metroMap.stations) {
				int mid = m.id;
				if (m.ref.equals("25906011")) {
					mid += 0;
				}
				
				Body mbody = system.stationBodies[mid];
				Vec2 mpos = mbody.getPosition();
				
				// Ensure m is in bounding box
				if (mpos.x < xMin || mpos.x > xMax || mpos.y < yMin || mpos.y > yMax) {
					calcCount1++;
					continue;
				}
				
				// Ensure station m is not part of edge e
				if (e.src.id == mid || e.dst.id == mid)
					continue;
				
				// Get an approximation by calculating the distance to the
				// convex triangle of the Bézier curve.
//				Vec2 approxProjection = triangleClosestPoint(mpos, v0, v1, cp);
//				float approxDistSq = approxProjection.lengthSquared();
//				if (approxDistSq < minDistSq) {
//					calcCount2++;
//					continue;
//				}
				
				if (m.ref.equals("25906011")) {
					mid += 0;
				}
				
				// Get Bézier projection
				calcCount3++;
				Vec2[] controlPoints = { v0, cp, v1 };
				Vec2 projection = BezierUtil.pointProjection(mpos, controlPoints);
				m.edgeProj.add(projection);
				Vec2 f = mpos.sub(projection);
				
				float dist = f.normalize();
				float distSq = dist * dist;
				
				float fmag = 0;
				if (baseWeight.value >= distSq * maxForce.value)
					fmag = (float) maxForce.value;
				else
					fmag = (float) (baseWeight.value / distSq);
				
				mbody.applyForceToCenter(f.mulLocal(fmag));
			}
		}
//
//		double sum = calcCount1 + calcCount2 + calcCount3;
//		System.out.println(String.valueOf(calcCount1/sum) + "\t" + String.valueOf(calcCount2/sum) + "\t" + String.valueOf(calcCount3/sum));
	}

	// TODO: replace this with a proper triangle projection function, that also
	// tests whether point is inside triangle.
	Vec2 triangleClosestPoint(Vec2 p, Vec2 u, Vec2 v, Vec2 w) {
		Vec2 uv = BasicGeometry.pointLineSegmentProjection(p, u, v);
		Vec2 vw = BasicGeometry.pointLineSegmentProjection(p, v, w);
		Vec2 wu = BasicGeometry.pointLineSegmentProjection(p, w, u);

		float distUv = uv.lengthSquared();
		float distVw = vw.lengthSquared();
		float distWu = wu.lengthSquared();

		if (distUv < distVw) {
			if (distUv < distWu)
				return uv;
			else
				return wu;
		} else {
			if (distVw < distWu)
				return vw;
			else
				return wu;
		}
	}
}
